//=========================================================================
// Copyright (C) 2024 The C++ Component Model(COMO) Open Source Project
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//=========================================================================

#include <stdio.h>
#include <math.h>
#include <memory.h>
#include <syslog.h>
#include "config.h"
#include "math_utils.h"

#ifdef RUN_ON_ARMv8
#include <arm_neon.h>
#endif

#ifdef RUN_ON_X86
#include <immintrin.h>  // AVX2 library (Only for x86)
#endif

#define VOTER_FLOAT_PRESION   0.0001

#define VOTER_DATA_TYPE       int
//#define VOTER_DATA_TYPE       float
//#define VOTER_DATA_TYPE       double

/**
 * 找出整数数组中的众数
 */
int FindMode(Int64 *arr, int size, int atleast, DataState *dataState)
{
    int maxCount = 0; // 最大出现次数
    int currentCount; // 当前数字出现次数
    int modeIdx = 0;

    // 遍历数组中的每个数字
    for (int i = 0;  i < size;  i++) {
        if (DATA_READY != dataState[i]) {
            continue;
        }

        currentCount = 0;

        // 统计当前数字在数组中出现的次数
        for (int j = 0;  j < size;  j++) {
            if (DATA_READY != dataState[j]) {
                continue;
            }

#if VOTER_DATA_TYPE == int
            if (arr[j].i64Val == arr[i].i64Val) {
#elif VOTER_DATA_TYPE == float
            if (fabs(arr[j].floatVal - arr[i].floatVal) < VOTER_FLOAT_PRESION) {
#elif VOTER_DATA_TYPE == double
            if (fabs(arr[j].doubleVal - arr[i].doubleVal) < VOTER_FLOAT_PRESION) {
#else
    #error Wrong value of macro VOTER_DATA_TYPE
#endif
                currentCount++;
            }
        }

        // 如果当前数字的出现次数大于最大出现次数，则更新最大出现次数和众数
        if (currentCount > maxCount) {
            maxCount = currentCount;
            modeIdx = i;
        }
    }

    if (maxCount < atleast) {
        return -1;
    }

    return modeIdx;
}

/**
 * 主成份算法
 */
int MajorityElement(Int64 *nums, int numsSize, int atleast, DataState *dataState)
{
    if ((NULL == nums) || (NULL == dataState) || (numsSize <= 0)) {
        return -1;
    }

    int count = 0;
    int modeIdx = -1;
    Int64 candidate;        // 给它任何初始值都是没意义的

    // 找出 candidate
    for (int i = 0;  i < numsSize;  i++) {
        if (DATA_READY != dataState[i]) {
            continue;
        }

        if (0 == count) {
            candidate = nums[i];
            modeIdx = i;
            count = 1;
        }
        else {
#if VOTER_DATA_TYPE == int
            if (candidate.i64Val == nums[i].i64Val)
#elif VOTER_DATA_TYPE == float
            if (fabs(candidate.floatVal - nums[i].floatVal) < VOTER_FLOAT_PRESION)
#elif VOTER_DATA_TYPE == double
            if (fabs(candidate.doubleVal - nums[i].doubleVal) < VOTER_FLOAT_PRESION)
#else
    #error "Wrong value of macro VOTER_DATA_TYPE"
#endif
            {
                count++;
            }
            else {
                count--;
            }
        }
    }

    // 二次验证：统计 candidate 实际出现次数
    int validCount = 0;
    for (int i = 0;  i < numsSize;  i++) {
        if (DATA_READY != dataState[i]) {
            continue;
        }
#if VOTER_DATA_TYPE == int
        if (candidate.i64Val == nums[i].i64Val)
#elif VOTER_DATA_TYPE == float
        if (fabs(candidate.floatVal - nums[i].floatVal) < VOTER_FLOAT_PRESION)
#elif VOTER_DATA_TYPE == double
        if (fabs(candidate.doubleVal - nums[i].doubleVal) < VOTER_FLOAT_PRESION)
#endif
        {
            validCount++;
        }
    }

    if (validCount < atleast) {
        return -1;
    }

    return modeIdx;
}

/**
 * 基于算术操作的比较 memcmp
 * 
 * 首先以uint64_t类型比较内存区域，然后逐字节比较
 * 
 * @param s1 第一个内存区域的起始地址
 * @param s2 第二个内存区域的起始地址
 * @param n  要比较的字节数
 * @return   如果两个内存区域相等，返回0；否则返回第一个不同字节的差值
 */
int memcmp_arithmetic(const void *s1, const void *s2, size_t n)
{
    // 将内存区域的起始地址转换为uint64_t类型的指针，以便进行比较。
    const uint64_t* p1 = (const uint64_t*)s1;
    const uint64_t* p2 = (const uint64_t*)s2;

    // 当剩余比较长度大于等于uint64_t大小时，进行uint64_t类型的比较
    while (n >= sizeof(uint64_t)) {
        // 如果找到不相等的uint64_t，进一步逐字节比较
        if (*p1 - *p2) {
            // 指针类型转换：uint64_t --> unsigned char，以便逐字节比较
            const unsigned char* c1 = (const unsigned char*)p1;
            const unsigned char* c2 = (const unsigned char*)p2;
            // 在uint64_t的范围内逐字节比较
            for (size_t i = 0;  i < sizeof(uint64_t);  i++) {
                // 如果找到不相等的字节，返回其差值
                if (c1[i] - c2[i]) {
                    return c1[i] - c2[i];
                }
            }
        }
        // 移动到下一个uint64_t并减少剩余比较长度
        ++p1;
        ++p2;
        n -= sizeof(uint64_t);
    }

    // 将剩余的内存区域转换为unsigned char类型的指针，以便逐字节比较
    const unsigned char* c1 = (const unsigned char*)p1;
    const unsigned char* c2 = (const unsigned char*)p2;
    // 逐字节比较剩余的内存区域
    while (n--) {
        // 如果找到不相等的字节，返回其差值
        if (*c1 - *c2) {
            return *c1 - *c2;
        }
        ++c1;
        ++c2;
    }

    // 所有比较的字节都相等，返回0
    return 0;
}

#ifdef RUN_ON_X86
/**
 * 基于算术操作的比较memcmp (使用AVX2并行计算)
 * 
 * 首先以uint64_t类型比较内存区域，然后逐字节比较
 * 
 * @param s1 第一个内存区域的起始地址
 * @param s2 第二个内存区域的起始地址
 * @param n  要比较的字节数
 * @return   如果两个内存区域相等，返回0；否则返回第一个不同字节的差值
 */
int memcmp_arithmetic_AVX2(const void *s1, const void *s2, size_t n)
{
    const unsigned char *c1 = (const unsigned char *)s1;
    const unsigned char *c2 = (const unsigned char *)s2;

    // 使用AVX2每次比较32字节
    while (n >= 32) {
        // 加载32字节
        __m256i vec1 = _mm256_loadu_si256((const __m256i *)c1);
        __m256i vec2 = _mm256_loadu_si256((const __m256i *)c2);

        // 比较两个向量
        __m256i result = _mm256_cmpeq_epi8(vec1, vec2);

        // 将相等结果转换为掩码
        unsigned int mask = _mm256_movemask_epi8(result);

        // 如果mask不是全1，则存在不相等的字节
        if (mask != 0xFFFFFFFFu) {
            // 找到第一个不相等的字节
            for (size_t i = 0;  i < 32;  i++) {
                if ((mask & (1 << i)) == 0) {
                    return c1[i] - c2[i];
                }
            }
        }

        // 移动到下一个32字节
        c1 += 32;
        c2 += 32;
        n -= 32;
    }

    // 处理剩余不足32字节的部分
    if (n > 0) {
        // 创建临时缓冲区并填充数据
        unsigned char temp1[32] = {0};
        unsigned char temp2[32] = {0};
        memcpy(temp1, c1, n);
        memcpy(temp2, c2, n);

        // 加载32字节
        __m256i vec1 = _mm256_loadu_si256((const __m256i *)temp1);
        __m256i vec2 = _mm256_loadu_si256((const __m256i *)temp2);

        // 比较两个向量
        __m256i result = _mm256_cmpeq_epi8(vec1, vec2);

        // 将相等结果转换为掩码
        unsigned int mask = _mm256_movemask_epi8(result);

        // 如果mask不是全1，则存在不相等的字节
        if (mask != 0xFFFFFFFFu) {
            // 找到第一个不相等的字节
            for (size_t i = 0;  i < n;  i++) {
                if ((mask & (1 << i)) == 0) {
                    return temp1[i] - temp2[i];
                }
            }
        }
    }

    // 所有比较的字节都相等，返回0
    return 0;
}
#endif

#ifdef RUN_ON_ARMv8
/**
 * 基于算术操作的比较memcmp (使用ASIMD并行计算)
 * 
 * @param s1 第一个内存区域的起始地址
 * @param s2 第二个内存区域的起始地址
 * @param n  要比较的字节数
 * @return   如果两个内存区域相等，返回0；否则返回第一个不同字节的差值
 */
int memcmp_arithmetic_ASIMD(const void *s1, const void *s2, size_t n)
{
    const unsigned char *c1 = (const unsigned char *)s1;
    const unsigned char *c2 = (const unsigned char *)s2;

    // 使用ASIMD每次比较16字节
    while (n >= 16) {
        // 加载16字节
        uint8x16_t vec1 = vld1q_u8(c1);
        uint8x16_t vec2 = vld1q_u8(c2);

        // 比较两个向量
        uint8x16_t result = vceqq_u8(vec1, vec2);

        // 生成每个字节的掩码
        uint64_t mask = vgetq_lane_u8(result, 0);
        mask |= (uint64_t)vgetq_lane_u8(result, 1) << 8;
        mask |= (uint64_t)vgetq_lane_u8(result, 2) << 16;
        mask |= (uint64_t)vgetq_lane_u8(result, 3) << 24;
        mask |= (uint64_t)vgetq_lane_u8(result, 4) << 32;
        mask |= (uint64_t)vgetq_lane_u8(result, 5) << 40;
        mask |= (uint64_t)vgetq_lane_u8(result, 6) << 48;
        mask |= (uint64_t)vgetq_lane_u8(result, 7) << 56;

        // 如果mask不是全1，则存在不相等的字节
        if (mask != 0xFFFFFFFFFFFFFFFFu) {
                   // 5432109876543210
            // 找到第一个不相等的字节
            for (size_t i = 0;  i < 16;  i++) {
                if ((mask & (1ull << i)) == 0) {
                    return c1[i] - c2[i];
                }
            }
        }

        // 移动到下一个16字节
        c1 += 16;
        c2 += 16;
        n -= 16;
    }

    // 处理剩余不足16字节的部分
    if (n > 0) {
        // 创建临时缓冲区并填充数据
        unsigned char temp1[16] = {0};
        unsigned char temp2[16] = {0};
        memcpy(temp1, c1, n);
        memcpy(temp2, c2, n);

        // 填充剩余部分为0
        temp1[n] = temp2[n] = 0;

        // 加载16字节
        uint8x16_t vec1 = vld1q_u8(temp1);
        uint8x16_t vec2 = vld1q_u8(temp2);

        // 比较两个向量
        uint8x16_t result = vceqq_u8(vec1, vec2);

        // 生成每个字节的掩码
        uint64_t mask = vgetq_lane_u8(result, 0);
        mask |= (uint64_t)vgetq_lane_u8(result, 1) << 8;
        mask |= (uint64_t)vgetq_lane_u8(result, 2) << 16;
        mask |= (uint64_t)vgetq_lane_u8(result, 3) << 24;
        mask |= (uint64_t)vgetq_lane_u8(result, 4) << 32;
        mask |= (uint64_t)vgetq_lane_u8(result, 5) << 40;
        mask |= (uint64_t)vgetq_lane_u8(result, 6) << 48;
        mask |= (uint64_t)vgetq_lane_u8(result, 7) << 56;

        // 如果mask不是全1，则存在不相等的字节
        if (mask != 0xFFFFFFFFFFFFFFFFu) {
                   // 5432109876543210
            // 找到第一个不相等的字节
            for (size_t i = 0;  i < n;  i++) {
                if ((mask & (1ull << i)) == 0) {
                    return temp1[i] - temp2[i];
                }
            }
        }
    }

    // 所有比较的字节都相等，返回0
    return 0;
}
#endif

/**
 * 1. 通过函数返回值 及 votingResult 参数，两个变量作为计算结果，防止函数调用过程中，
 * 内存或寄存器出错，造成对计算结果使用的错误
 * 2. 真假值通过具有足够码距的编码来表示：
 *    HEX: 55AA == BIN: 0101010110101010
 *    HEX: AA55 == BIN: 1010101001010101
 */
int BigDataVoter(BigVotingData *arr, size_t arrSize, size_t dataSize,
                                     DataState *dataState, int atleastClients,
                                     VOTING_RESULT *votingResult)
{
    int cmpResult_0, cmpResult_1, cmpResult_2;
    int32_t match_counts[MAX_CLIENTS];

    if ((arrSize > MAX_CLIENTS) || (NULL == arr) ||
                                (NULL == dataState) || (NULL == votingResult)) {

        *votingResult = VOTING_FAIL;
        return -1;
    }

    int num = 0;
    for (size_t i = 0;  i < arrSize;  i++) {
        if (DATA_READY == dataState[i]) {
            num++;
        }
    }

    if (num < atleastClients) {
        *votingResult = VOTING_FAIL;
        return -2;
    }

    if (atleastClients < 2) {
        for (size_t i = 0;  i < arrSize;  i++) {
            if (DATA_READY == dataState[i]) {
                *votingResult = VOTING_PASS;
                return (int)i;
            }
        }

        return -3;
    }

    memset(match_counts, '\0', sizeof(int32_t) * MAX_CLIENTS);

    int32_t majority_threshold;
    if (g_pconfig->N_in_NooM > 0) {
        majority_threshold = g_pconfig->N_in_NooM - 1;
    }
    else {
        majority_threshold = dataSize / 2 - 1;
    }

    for (size_t i = 0;  i < arrSize;  i++) {
        for (size_t j = i + 1;  j < arrSize;  j++) {
            if (DATA_READY != dataState[j]) {
                continue;
            }

            // 如果这个包已经被确定为多数，跳过比较
            if (match_counts[i] > (int32_t)arrSize / 2) {
                continue;
            }

            // 算法1
            cmpResult_0 = memcmp_arithmetic(arr[0].bytes, arr[i].bytes, dataSize);
            // 算法2
            cmpResult_1 = memcmp(arr[0].bytes, arr[i].bytes, dataSize);

            cmpResult_2 = 0;
            // 算法3
#ifdef RUN_ON_X86
            cmpResult_2 = memcmp_arithmetic_AVX2(arr[0].bytes, arr[i].bytes, dataSize);
#endif
#ifdef RUN_ON_ARMv8
            cmpResult_2 = memcmp_arithmetic_ASIMD(arr[0].bytes, arr[i].bytes, dataSize);
#endif
            if ((0 == cmpResult_0) && (0 == cmpResult_1) && (0 == cmpResult_2)) {
                match_counts[i]++;
                match_counts[j]++;
            }
        }
    }

    *votingResult = VOTING_FAIL;

    // 查找多数
    for (size_t i = 0;  i < arrSize;  i++) {
        if (match_counts[i] >= majority_threshold) {
            *votingResult = VOTING_PASS;
            return (int)i;
        }
    }

    if (VOTING_FAIL == *votingResult) {
        return -3;
    }

    return 0;
}

/**
 * 计算循环距离（考虑回绕）
 * 返回值范围: 0 ~ N
 */
uint64_t distance_mod(uint64_t prev, uint64_t curr, uint64_t N)
{
    if (0 == N) {
        return 0;
    }

    return (curr + N + 1 - prev) % (N + 1);
}

/**
 * 判断 curr 是否是 prev 的后一个（考虑回绕与步长 step）
 */
bool is_next_range(uint64_t prev, uint64_t curr,
                   uint64_t N, uint64_t step_min, uint64_t step_max)
{
    if ((0 == N) || (step_min > step_max)) {
        return false;
    }

    uint64_t range = N + 1;
    uint64_t diff = (curr + range - prev) % range;

    return ((diff >= step_min) && (diff <= step_max));
}

/**
 * 模拟计数器递增
 */
uint64_t counter_next(uint64_t current, uint64_t N, uint64_t step)
{
    return (current + step) % (N + 1);
}
